import rospy
from sensor_msgs.msg import JointState
from geometry_msgs.msg import Vector3
from scipy.optimize import fsolve
import numpy as np
import math
class Move:
    def __init__(self):
        self.l = 0.4
        self.rho = 0
        self.alpha = 0
        self.z = 0
        self.names = ["spin","arm1","arm2","arm3","finger1","finger2"]
        self.state = []

        rospy.init_node("Move")
        # self.ROBOT = rospy.get_param("~robot")
        self.pub = rospy.Publisher("/joint_states",JointState,queue_size=10)

        rospy.Subscriber("/position",Vector3,self.call_back)
        rospy.spin()
    def spawn_func(self):
        def func(alpha):
            alpha0 = alpha[0]
            alpha1 = alpha[1]
            l = self.l
            eq1 = l + l*math.cos(alpha0-alpha1)+l*math.cos(alpha0) - self.rho
            eq2 = l*math.sin(alpha0) + l*math.sin(alpha0-alpha1) - self.z
            eq = np.array([eq1,eq2])
            return eq
        return func

    def change_position(self,rho,alpha,z):
        self.rho = rho
        self.alpha = alpha
        self.z = z
    
    def check(self):
        self.RHO_RANGE_H = 3*self.l
        self.RHO_RANGE_L = self.l
        self.Z_RANGE = 2*self.l
        self.ALPHA_RANGE = 3.1416

        if abs(self.alpha) > self.ALPHA_RANGE:
            print("alpha error")
            return 1 

        if self.rho>self.RHO_RANGE_H or self.rho<self.RHO_RANGE_L:
            print("rho error")
            return 2
        return 0

    def call_back(self,data:Vector3):
        msg = JointState()
        self.rho = data.x
        self.alpha = data.y
        self.z = data.z
        if self.check() != 0:
            return 
        f = self.spawn_func()
        result = fsolve(f,[0,0])
        self.state = result/180
        print(result)

        for name in self.names:
            msg.name.append(name+"_joint") 
        msg.position=[0,0,0,0,0,0]
        msg.position[0] = self.alpha
        msg.position[1] = 1.5708-self.state[0]
        msg.position[2] = self.state[1]
        msg.position[3] = self.state[0] - self.state[1]

        self.pub.publish(msg)




if __name__ == "__main__":
    controller = Move()